#pragma once
#include <Eigen/Dense>
#include <map>
namespace auto_ros
{
namespace control
{
typedef unsigned int uint;
class LinearSys
{
public:
	LinearSys(uint state_dim, uint control_dim, uint distur_dim = 0, uint out_dim = 0);
	bool set_state_matrix(const Eigen::MatrixXd state_matrix);
	bool set_control_matrix(const Eigen::MatrixXd control_matrix);
	bool set_distur_matrix(const Eigen::MatrixXd set_distur_matrix);
	bool set_out_state_matrix(const Eigen::MatrixXd out_state_matrix);
	bool set_out_distur_matrix(const Eigen::MatrixXd out_distur_matrix);

	bool set_matrix(const Eigen::MatrixXd state_matrix,
					const Eigen::MatrixXd control_matrix,
					const Eigen::MatrixXd set_distur_matrix = Eigen::MatrixXd::Zero(4, 0));
	Eigen::MatrixXd state_matrix();
	Eigen::MatrixXd control_matrix();
	Eigen::MatrixXd distur_matrix();
	Eigen::MatrixXd out_state_matrix();
	Eigen::MatrixXd out_distur_matrix();

	uint control_dim();
	uint state_dim();
	uint distur_dim();
	uint out_dim();
	bool is_state_matrix_changed();
	bool is_control_matrix_changed();
	bool is_distur_matrix_changed();
	Eigen::MatrixXd state_matrix_;
	Eigen::MatrixXd control_matrix_;
	Eigen::MatrixXd distur_matrix_;
	Eigen::MatrixXd out_state_matrix_;
	Eigen::MatrixXd out_distur_matrix_;

protected:
	friend class Mpc;
	friend class CvxMpcBase;
	friend class LinearSteadyState;
	uint state_dim_;
	uint control_dim_;
	uint distur_dim_;
	uint out_dim_;

	bool is_state_matrix_changed_ = true;

	bool is_control_matrix_changed_ = true;

	bool is_distur_matrix_changed_ = true;
};
} // namespace control
} // namespace auto_ros